#include "unitree_arm_sdk/control/unitreeArm.h"
#include <iostream>
#include <unistd.h>
#include <sys/socket.h>
#include <netinet/in.h>
#include <arpa/inet.h>
#include <cstring>
#include <iomanip>
#include <thread>
#include <mutex>
#include <condition_variable>
#include <atomic>
#include <fcntl.h>
#include <sys/select.h>
#include <fstream>
#include <chrono>
#include <ctime>
#include <sstream>
#include <string>
#include <cerrno>
#include <termios.h>
#include "unitree_arm_sdk/thirdparty/robotics.h"    // 假设这里有rpyxyz到HomoMat的工具函数
#include "unitree_arm_sdk/model/ArmModel.h"         // 包含ArmModel、Z1Model声明
#include <iomanip>
#include <cstring>
#include <map>

// =============== 你原先的开关 ===============
// #define USE_SPEED_CONTROL
#define USE_POSITION_CONTROL
#define thresh 0.2
#define DELAY 150
// #define USE_PREDICTIVE_CONTROL

// =============== 新增：外层导纳控制开关 ===============
// 如果你想测试“外层力控(导纳)”，可改为：
#undef  USE_POSITION_CONTROL
#define USE_ADMITTANCE_CONTROL


using namespace UNITREE_ARM;

// ===== 改为“只处理最新数据”，故不用顺序队列了 =====
// 全局仅存一个“最新packet”和互斥量
static std::mutex g_latestMutex;
static std::vector<unsigned char> g_latestPacket;
static std::atomic<bool> g_hasNewPacket(false);
static Vec6 g_serverDesPosture = Vec6::Zero();

float force[6] = {0.0f}; // 全局力数据 (Fx,Fy,Fz,Tx,Ty,Tz)
std::mutex force_mutex;
std::mutex posture_mutex;   // 用来保护 g_serverDesPosture
std::condition_variable force_cv;
std::atomic<bool> exit_flag(false);
std::atomic<bool> s_trigger(false);  // 用来让导纳线程检测“s”键的触发
// 用于在各线程间共享“是否暂停”的状态
std::atomic<bool> s_pause(false);
std::mutex arm_mutex;
// 用来告诉电机线程要执行哪种操作
enum class MotorCommand {
    NONE,
    ENABLE,   // 对应按键 t
    DISABLE   // 对应按键 d
};
// 全局原子变量，表示当前需要给电机发送的指令
std::atomic<MotorCommand> g_motorCmd(MotorCommand::NONE);
std::atomic<bool> exit_flag_motor;
// 串口文件描述符 (可全局或放在线程里)
static int motor_serial_fd = -1;
uint16_t crc16_modbus(const uint8_t *data, size_t len) {
    uint16_t crc = 0xFFFF;
    for (size_t i = 0; i < len; i++) {
        crc ^= (uint16_t)data[i];
        for (int j = 0; j < 8; j++) {
            if (crc & 0x0001) {
                crc >>= 1;
                crc ^= 0xA001;
            } else {
                crc >>= 1;
            }
        }
    }
    return crc;
}

using namespace std;

#include <fcntl.h>
#include <termios.h>
#include <unistd.h>
#include <errno.h>
#include <cstring>
#include <iostream>

/**
 * @brief 打开并初始化电机串口
 * @param portname 串口设备名，如 "/dev/ttyUSB1"
 * @param baudrate 波特率, 例如 B115200
 * @return 成功返回 fd，失败返回 -1
 */
int openMotorSerial(const std::string &portname, speed_t baudrate) {
    int fd = open(portname.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
    if (fd < 0) {
        std::cerr << "[openMotorSerial] open " << portname 
                  << " failed: " << strerror(errno) << std::endl;
        return -1;
    }

    // 配置
    struct termios tty;
    memset(&tty, 0, sizeof(tty));
    if (tcgetattr(fd, &tty) != 0) {
        std::cerr << "[openMotorSerial] tcgetattr error: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }

    cfsetispeed(&tty, baudrate);
    cfsetospeed(&tty, baudrate);
    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;  // 8-bit chars
    tty.c_iflag &= ~IGNBRK;
    tty.c_lflag = 0;
    tty.c_oflag = 0;
    tty.c_cc[VMIN]  = 0;
    tty.c_cc[VTIME] = 5;

    tty.c_iflag &= ~(IXON | IXOFF | IXANY);  // no sw flow control
    tty.c_cflag |= (CLOCAL | CREAD);
    tty.c_cflag &= ~(PARENB | PARODD);       // no parity
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CRTSCTS;                // no hw flow control

    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        std::cerr << "[openMotorSerial] tcsetattr error: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }
    return fd;
}

/**
 * @brief  将形如 "41540007e84c01000d0a" 的 hex 字符串转换为字节后发送
 */
bool sendHexCommand(int fd, const std::string &hexStr) {
    if(fd < 0) {
        std::cerr << "[sendHexCommand] invalid fd\n";
        return false;
    }

    // 1) 把hexStr转成字节
    //    每两个hex字符表示一个字节
    //    比如 "41" -> 0x41
    if(hexStr.size() % 2 != 0) {
        std::cerr << "[sendHexCommand] hexStr length error\n";
        return false;
    }
    std::vector<uint8_t> data;
    data.reserve(hexStr.size() / 2);

    for(size_t i = 0; i < hexStr.size(); i += 2) {
        // 取两个hex字符
        std::string byteString = hexStr.substr(i, 2);
        // 转为数值
        uint8_t val = (uint8_t) strtoul(byteString.c_str(), nullptr, 16);
        data.push_back(val);
    }

    // 2) 发送到串口
    ssize_t n = write(fd, data.data(), data.size());
    if(n < 0) {
        std::cerr << "[sendHexCommand] write error: " << strerror(errno) << std::endl;
        return false;
    }
    tcflush(fd, TCOFLUSH);
    return (n == (ssize_t) data.size());
}

void motorSerialThreadFunc()
{
    // 1) 打开串口
    motor_serial_fd = openMotorSerial("/dev/ttyUSB1", B921600); // 端口&波特率按自己需求
    if(motor_serial_fd < 0) {
        std::cerr << "[motorSerialThreadFunc] openMotorSerial fail.\n";
        return; // 无法打开就直接返回
    }
    std::cout << "[motorSerialThreadFunc] motor serial opened.\n";

    // 2) 初始化时先发送两条指令
    sendHexCommand(motor_serial_fd, "41542b41540d0a"); 
    

    std::cout << "[motorSerialThreadFunc] init commands sent.\n";

    // 3) 进入循环，按需要定期检测指令 & 发送
    while(!exit_flag_motor) {
        // 检查是否有新命令
        MotorCommand cmd = g_motorCmd.load();
        if(cmd == MotorCommand::ENABLE) {
            // 发送使能
            sendHexCommand(motor_serial_fd, "41549007e80c080a700000000030c20d0a");
            sleep(1);
            sendHexCommand(motor_serial_fd, "41541807e80c0800000000000000000d0a");
            std::cout << "[motorSerialThreadFunc] Sent ENABLE.\n";
            // 清除命令
            g_motorCmd.store(MotorCommand::NONE);
        }
        else if(cmd == MotorCommand::DISABLE) {
            // 发送失能
            sendHexCommand(motor_serial_fd, "41542007e80c0800000000000000000d0a");
            std::cout << "[motorSerialThreadFunc] Sent DISABLE.\n";
            g_motorCmd.store(MotorCommand::NONE);
        }
        // 如果要读取返回的数据，也可在这里加 read 逻辑
        // ...

        // 稍微sleep一下避免空转
        std::this_thread::sleep_for(std::chrono::milliseconds(50));
    }

    // 循环结束，关闭串口
    close(motor_serial_fd);
    motor_serial_fd = -1;
    std::cout << "[motorSerialThreadFunc] motor serial closed, thread exit.\n";
}

void print_header_check(const unsigned char* response, size_t len) {
    // Print the first two bytes in Hex format
    if (len >= 2) {
        std::cout << "First 2 bytes: ";
        for (int i = 0; i < 2; i++) {
            std::cout << hex << uppercase << setw(2) << setfill('0') << (int)response[i] << " ";
        }
        cout << "(Hex)" << endl;
        cout << "Response Length: " << len << endl;
    }
}

void parse_response(const unsigned char* response, size_t len) {
    // Check if response length is valid
    if (len < 27) {
        cout << "Invalid response length." << endl;
        return;
    }

    // print_header_check(response, len);  // Print first two bytes for debugging

    // Header check (0xAA, 0x55)
    if (response[0] != 0xAA || response[1] != 0x55) {
        cout << "Invalid header." << endl;
        return;
    }

    // Get package length (2 bytes)
    unsigned int package_length = (response[2] << 8) | response[3];

    // Get package number (2 bytes)
    unsigned int package_no = (response[4] << 8) | response[5];

    // Get the data part (24 bytes)
    unsigned char data[24];
    memcpy(data, response + 6, 24);

    // Convert each channel data (4 bytes per channel) to a float
    for (int i = 0; i < 6; i++) {
        float value;
        memcpy(&value, data + i * 4, 4);
        force[i] = value;
    }
    // debug: force[i] 就是力和力矩

    // Get the checksum (last byte) [可忽略或自行校验]
    unsigned char checksum = response[len - 1];
}

int initSerialPort(const std::string& portname) {
    int fd = open(portname.c_str(), O_RDWR | O_NOCTTY | O_SYNC);
    if (fd < 0) {
        std::cerr << "[initSerialPort] Error opening " << portname << ": " << strerror(errno) << std::endl;
        return -1;
    }

    struct termios tty;
    memset(&tty, 0, sizeof tty);
    if (tcgetattr(fd, &tty) != 0) {
        std::cerr << "[initSerialPort] Error from tcgetattr: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }

    cfsetospeed(&tty, B115200);
    cfsetispeed(&tty, B115200);

    tty.c_cflag = (tty.c_cflag & ~CSIZE) | CS8;
    tty.c_iflag &= ~IGNBRK;
    tty.c_lflag = 0;
    tty.c_oflag = 0;
    tty.c_cc[VMIN]  = 0;
    tty.c_cc[VTIME] = 5;
    tty.c_iflag &= ~(IXON | IXOFF | IXANY);
    tty.c_cflag |= (CLOCAL | CREAD);
    tty.c_cflag &= ~(PARENB | PARODD);
    tty.c_cflag &= ~CSTOPB;
    tty.c_cflag &= ~CRTSCTS;

    if (tcsetattr(fd, TCSANOW, &tty) != 0) {
        std::cerr << "[initSerialPort] Error from tcsetattr: " << strerror(errno) << std::endl;
        close(fd);
        return -1;
    }
    return fd;
}

class Z1ARM : public unitreeArm {
public:
    Z1ARM() : unitreeArm(true), tcp_sockfd(-1) {};
    ~Z1ARM() {
        if(tcp_sockfd != -1){
            close(tcp_sockfd);
            std::cout << "[~Z1ARM] TCP socket closed." << std::endl;
        }
    };

    void armCtrlByFSM();
    void armCtrlInJointCtrl();
    void armCtrlInCartesian();
    void printState();

    double gripper_pos = 0.0;
    double joint_speed = 2.0;
    double cartesian_speed = 3.0;

    void setForwardPosition(const Vec6& pos) { forward_position = pos; }
    Vec6 getForwardPosition() const { return forward_position; }

    int tcp_sockfd;

private:
    Vec6 forward_position;
};

void Z1ARM::armCtrlByFSM() {
    std::cout << "[armCtrlByFSM] Before labelRun(\"forward\")" << std::endl;
    try {
        labelRun("forward");
        std::cout << "[armCtrlByFSM] labelRun(\"forward\") OK." << std::endl;
    }
    catch (...) {
        std::cerr << "[armCtrlByFSM] Exception in labelRun." << std::endl;
        return;
    }

    std::cout << "[armCtrlByFSM] After labelRun" << std::endl;
    printState();

    {
        std::lock_guard<std::mutex> lock(arm_mutex);
        forward_position = lowstate->endPosture;
        std::cout << "[armCtrlByFSM] Save forward_position: " << forward_position.transpose() << std::endl;
    }
}

void Z1ARM::armCtrlInJointCtrl(){
    std::cout << "[armCtrlInJointCtrl] Before labelRun(\"forward\")" << std::endl;
    try {
        labelRun("forward");
        std::cout << "[armCtrlInJointCtrl] labelRun(\"forward\") OK." << std::endl;
    }
    catch (...) {
        std::cerr << "[armCtrlInJointCtrl] Exception in labelRun." << std::endl;
        return;
    }

    std::cout << "[armCtrlInJointCtrl] After labelRun" << std::endl;
    startTrack(ArmFSMState::JOINTCTRL);

    Timer timer(_ctrlComp->dt);
    for(int i = 0; i < 1000; i++){
        if (exit_flag) {
            std::cout << "[armCtrlInJointCtrl] Exit flag set, break." << std::endl;
            break;
        }
        directions << 0, 0, 0, -1, 0, 0, -1;
        joint_speed = 1.0;
        jointCtrlCmd(directions, joint_speed);
        timer.sleep();
    }
    std::cout << "[armCtrlInJointCtrl] Completed." << std::endl;
}

void Z1ARM::armCtrlInCartesian(){
    std::cout << "[armCtrlInCartesian] Before labelRun(\"forward\")" << std::endl;
    try {
        labelRun("forward");
        std::cout << "[armCtrlInCartesian] labelRun(\"forward\") OK." << std::endl;
    }
    catch (...) {
        std::cerr << "[armCtrlInCartesian] Exception in labelRun." << std::endl;
        return;
    }

    std::cout << "[armCtrlInCartesian] After labelRun" << std::endl;
    startTrack(ArmFSMState::CARTESIAN);

    double angular_vel = 0.3;
    double linear_vel = 0.3;
    Timer timer(_ctrlComp->dt);
    for(int i = 0; i < 2000; i++){
        if (exit_flag) {
            std::cout << "[armCtrlInCartesian] Exit flag set, break." << std::endl;
            break;
        }
        directions << 0, 0, 0, 0, 0, -1, -1;
        cartesianCtrlCmd(directions, angular_vel, linear_vel);
        timer.sleep();
    }
    std::cout << "[armCtrlInCartesian] Completed." << std::endl;
}

void Z1ARM::printState()
{
    std::cout << "------ joint State ------" << std::endl;
    std::cout << "qState: " << lowstate->getQ().transpose() << std::endl;
    std::cout << "qdState: " << lowstate->getQd().transpose() << std::endl;
    std::cout << "tauState: " << lowstate->getTau().transpose() << std::endl;
    std::cout << "------ Endeffector Cartesian Posture ------" << std::endl;
    
    // 设置输出精度为小数点后6位
    std::cout << std::fixed << std::setprecision(6);
    std::cout << "(roll pitch yaw x y z) = " << lowstate->endPosture.transpose() << std::endl;
    
    // 恢复默认精度（可选）
    std::cout << std::defaultfloat;
}

void monitorKeyboard(std::atomic<bool>& exit_flag) {
    std::cout << "[monitorKeyboard] Press 'q' to exit.\n"
              << "Press 't' to ENABLE motor.\n"
              << "Press 'd' to DISABLE motor.\n";

    char c;
    while (std::cin >> c) {
        if (c == 'q' || c == 'Q') {
            exit_flag = true;
            exit_flag_motor = true;  // <-- 新增这一行
            std::cout << "[monitorKeyboard] 'q' pressed. Exit...\n";
            force_cv.notify_all();
            break;
        }
        else if (c == 't' || c == 'T') {
            // 告诉电机线程：ENABLE
            g_motorCmd.store(MotorCommand::ENABLE);
            std::cout << "[monitorKeyboard] 't' pressed => motor ENABLE command queued.\n";
        }
        else if (c == 'd' || c == 'D') {
            // 告诉电机线程：DISABLE
            g_motorCmd.store(MotorCommand::DISABLE);
            std::cout << "[monitorKeyboard] 'd' pressed => motor DISABLE command queued.\n";
        }
    }
}



void tcpClient(Z1ARM& arm) {
    arm.tcp_sockfd = socket(AF_INET, SOCK_STREAM, 0);
    if (arm.tcp_sockfd < 0) {
        std::cerr << "[tcpClient] Failed to create socket: " << strerror(errno) << std::endl;
        return;
    }

    struct sockaddr_in server_addr;
    server_addr.sin_family = AF_INET;
    server_addr.sin_port = htons(14532);
    server_addr.sin_addr.s_addr = inet_addr("115.236.153.174");

    std::cout << "[tcpClient] Attempt to connect..." << std::endl;

    fd_set set;
    FD_ZERO(&set);
    FD_SET(arm.tcp_sockfd, &set);

    struct timeval timeout;
    timeout.tv_sec = 5;
    timeout.tv_usec = 0;

    int ret = connect(arm.tcp_sockfd, (struct sockaddr*)&server_addr, sizeof(server_addr));
    if (ret < 0) {
        if (errno == EINPROGRESS) {
            ret = select(arm.tcp_sockfd + 1, NULL, &set, NULL,  &timeout);
            if (ret <= 0) {
                std::cerr << "[tcpClient] Connect timed out or err: " << strerror(errno) << std::endl;
                close(arm.tcp_sockfd);
                arm.tcp_sockfd = -1;
                return;
            } else {
                int so_error = 0;
                socklen_t len = sizeof(so_error);
                getsockopt(arm.tcp_sockfd, SOL_SOCKET, SO_ERROR, &so_error, &len);
                if (so_error != 0) {
                    std::cerr << "[tcpClient] Connect fail so_error="<<so_error<<", " << strerror(so_error) << std::endl;
                    close(arm.tcp_sockfd);
                    arm.tcp_sockfd = -1;
                    return;
                }
            }
        } else {
            std::cerr << "[tcpClient] Connect fail: "<< strerror(errno) <<std::endl;
            close(arm.tcp_sockfd);
            arm.tcp_sockfd = -1;
            return;
        }
    }
    std::cout << "[tcpClient] Connected ok." << std::endl;
}

void sendForceData(Z1ARM& arm, float* force) {
    if (arm.tcp_sockfd == -1) {
        std::cerr << "[sendForceData] TCP not connected." << std::endl;
        return;
    }

    uint8_t packet[4 + 4 * 6 + 2]; // Adjusted to include all force channels (6 values)
    packet[0] = 0xAA;
    packet[1] = 0xBB;
    packet[2] = 0xCC;
    packet[3] = 0xDD;

    std::memcpy(&packet[4], force, sizeof(float) * 6);  // Copy the force array to the packet

    uint16_t crc = crc16_modbus(packet, 4 + 6 * 4);  // Adjusted to include the entire force data
    packet[4 + 6 * 4] = (uint8_t)(crc & 0xFF);       // CRC low byte
    packet[5 + 6 * 4] = (uint8_t)((crc >> 8) & 0xFF);  // CRC high byte

    ssize_t sent = send(arm.tcp_sockfd, (char*)packet, sizeof(packet), 0);
    if (sent < 0) {
        std::cerr << "[sendForceData] Failed: " << strerror(errno) << std::endl;
    }
}

void logArmPosition(Z1ARM& arm, std::atomic<bool>& exit_flag, const std::string& filename) {
    std::ofstream outfile(filename, std::ios::out);
    if (!outfile.is_open()) {
        std::cerr << "[logArmPosition] Cannot open file: " << filename << std::endl;
        return;
    }

    std::cout << "[logArmPosition] Logging posture to " << filename << std::endl;
    outfile << "timestamp_ms roll pitch yaw x y z Fx Fy Fz Tx Ty Tz "
            << "q0 q1 q2 q3 q4 q5 qd0 qd1 qd2 qd3 qd4 qd5 tau0 tau1 tau2 tau3 tau4 tau5\n";
    outfile.flush();

    std::chrono::milliseconds interval(10);
    while (!exit_flag) {
        auto now = std::chrono::system_clock::now();
        auto now_ms = std::chrono::duration_cast<std::chrono::milliseconds>(now.time_since_epoch()) % 1000;

        auto now_time_t = std::chrono::system_clock::to_time_t(now);
        std::tm tm_time;
        localtime_r(&now_time_t, &tm_time);

        std::ostringstream timestamp_stream;
        timestamp_stream << std::put_time(&tm_time, "%Y-%m-%d_%H-%M-%S") << '.'
                         << std::setfill('0') << std::setw(3) << now_ms.count();

        Vec6 endPosture;
        {
            std::lock_guard<std::mutex> lk(arm_mutex);
            if (arm.lowstate == nullptr) {
                std::cerr << "[logArmPosition] arm.lowstate=nullptr\n";
                endPosture = Vec6::Zero();
            } else {
                endPosture = arm.lowstate->endPosture;
            }
        }

        Vec6 Q = arm.lowstate->getQ();
        Vec6 Qd = arm.lowstate->getQd();
        Vec6 Qt = arm.lowstate->getTau();

        float force_vals[6];
        {
            std::lock_guard<std::mutex> lk(force_mutex);
            std::copy(std::begin(force), std::end(force), std::begin(force_vals));
        }

        outfile << timestamp_stream.str() << " "
                << endPosture[0] << " " << endPosture[1] << " " << endPosture[2] << " "
                << endPosture[3] << " " << endPosture[4] << " " << endPosture[5] << " "
                << force_vals[0] << " " << force_vals[1] << " " << force_vals[2] << " "
                << force_vals[3] << " " << force_vals[4] << " " << force_vals[5] << " "
                << Q[0] << " " << Q[1] << " " << Q[2] << " " << Q[3] << " " << Q[4] << " " << Q[5] << " "
                << Qd[0] << " " << Qd[1] << " " << Qd[2] << " " << Qd[3] << " " << Qd[4] << " " << Qd[5] << " "
                << Qt[0] << " " << Qt[1] << " " << Qt[2] << " " << Qt[3] << " " << Qt[4] << " " << Qt[5] << "\n";
        outfile.flush();

        std::this_thread::sleep_for(interval);
    }

    outfile.close();
    std::cout << "[logArmPosition] Logging stopped.\n";
}

// 在代码开头处修改延迟定义
// #define DELAY 200   // 旧的延迟定义
#define FIXED_NETWORK_DELAY 300  // 新的固定网络延迟，单位ms

// 修改 receiveDataFromServer 函数中的延迟处理部分
void receiveDataFromServer(Z1ARM& arm) {
    std::vector<unsigned char> buffer;
    const int PACKET_SIZE = 40;
    // 使用非阻塞或者阻塞都可以。这里如果你想阻塞读取，也行。
    // 为演示保持和你原先一样
    int flags = fcntl(arm.tcp_sockfd, F_GETFL, 0);
    if (flags == -1) {
        std::cerr << "[receiveDataFromServer] fcntl get err.\n";
        exit_flag = true; 
        return;
    }
    if (fcntl(arm.tcp_sockfd, F_SETFL, flags | O_NONBLOCK) == -1) {
        std::cerr << "[receiveDataFromServer] fcntl set O_NONBLOCK err.\n";
        exit_flag = true; 
        return;
    }

    while (!exit_flag) {
        // 1) 读数据
        unsigned char tmp[512];
        ssize_t bytes = recv(arm.tcp_sockfd, (char*)tmp, sizeof(tmp), 0);
        if (bytes < 0) {
            if (errno == EWOULDBLOCK || errno == EAGAIN) {
                // 没数据就稍微等一下
                std::this_thread::sleep_for(std::chrono::milliseconds(10));
                continue;
            } else {
                std::cerr << "[receiveDataFromServer] recv err: " << strerror(errno) << "\n";
                exit_flag = true; 
                break;
            }
        } else if (bytes == 0) {
            std::cerr << "[receiveDataFromServer] remote closed.\n";
            exit_flag = true; 
            break;
        }

        // 2) 追加到 buffer
        buffer.insert(buffer.end(), tmp, tmp + bytes);

        // 3) 检查是否有完整包(40字节)
        while (buffer.size() >= PACKET_SIZE && !exit_flag) {
            // 找头部
            bool found_header = false;
            size_t pos = buffer.size();
            for (size_t i = 0; i <= buffer.size() - PACKET_SIZE; i++) {
                if (buffer[i] == 0xAA && buffer[i+1] == 0xBB &&
                    buffer[i+2] == 0xCC && buffer[i+3] == 0xDD) {
                    pos = i;
                    found_header = true;
                    break;
                }
            }
            if(!found_header){
                buffer.clear();
                break;
            }

            // 4) 若有完整的 40字节包
            if (buffer.size() - pos >= PACKET_SIZE) {
                // 取出完整包
                std::vector<unsigned char> packet(buffer.begin()+pos,
                                                  buffer.begin()+pos+PACKET_SIZE);

                // a) 模拟固定时延: 收到包后先睡眠 300ms
                std::this_thread::sleep_for(std::chrono::milliseconds(FIXED_NETWORK_DELAY));

                // b) 解析
                //    如果还要往 [40..43] 写 delay_ms，可以写个固定值
                //    例如 int32_t fakeDelay = FIXED_NETWORK_DELAY;
                //    packet.resize(44);
                //    memcpy(&packet[40], &fakeDelay, sizeof(int32_t));

                // c) 存到 g_latestPacket
                {
                    std::lock_guard<std::mutex> lk(g_latestMutex);
                    g_latestPacket = packet;
                    g_hasNewPacket.store(true);
                }

                // 把已经处理的部分从 buffer 中丢弃
                buffer.erase(buffer.begin(), buffer.begin() + pos + PACKET_SIZE);
            } else {
                break; // 不够 40字节
            }
        }
    }
    std::cout<<"[receiveDataFromServer] thread exit.\n";
}


// ============= 解析完的目标姿态，给 g_serverDesPosture =============
/*
 * parseServerThreadFunc
 *  - 和你原先 armControlThreadFunc 类似，但是只做“解析服务器包 -> 末端姿态映射”
 *  - 不再做 IK/插值等操作
 */
void parseServerThreadFunc(){

    while(!exit_flag){
        // 如果没有新的包就等一会
        if(!g_hasNewPacket.load()){
            std::this_thread::sleep_for(std::chrono::milliseconds(30));
            continue;
        }
        std::vector<unsigned char> raw_packet;
        {
            std::lock_guard<std::mutex> lk(g_latestMutex);
            raw_packet = g_latestPacket;
            g_hasNewPacket.store(false);
        }
        if(raw_packet.size() < 44) {
            continue;
        }
        
        float pos_array[7];
        memcpy(pos_array,&raw_packet[4],7*sizeof(float));
        
        double x= pos_array[0];
        double y= pos_array[1];
        double z= pos_array[2];
        double pitch= pos_array[3];
        double yaw=   pos_array[4];
        double roll=  pos_array[5];
        double pen =  pos_array[6]; // maybe for microadjust or something
        
        // 这里就是你原先的"拟合/映射逻辑"
        // 例如: new_position[3] = 4*(z/1000.0f)
        //       ...
        Vec6 newPos;
        newPos[0] = roll/1.0;    // r
        newPos[1] = pitch/1.0;   // p
        newPos[2] = yaw/1.0;     // y
        newPos[3] = 4*(z/1000.0); // x
        // newPos[3] = 4*(z/1000.0)+0.04; // x
        newPos[4] = 4*(x/1000.0); // y
        newPos[5] = 2*(y/1000.0) + 0.31; // z

        if(pen>thresh){
           /*drill*/ 
            newPos[0] = 0;    // r
            newPos[1] = 0;   // p
            newPos[2] = 0;     // y
            // newPos[3] = 0.3+2*(z/1000.0)/5; // x
            newPos[3] = 0.3+2*(z/1000.0)/3; // x
            newPos[4] = 0; // y
            newPos[5] = 2*(25/1000.0) + 0.31; // z
            /*grind*/
            // newPos[0] = 0;    // r
            // newPos[1] = 0;   // p
            // newPos[2] = 0;     // y
            // newPos[3] = 0.3+2*(z/1000.0)/5; // x
            // newPos[4] = 2*(x/1000.0)/5; // y
            // newPos[5] = 2*(20/1000.0+y/1000/5) + 0.31; // z
            /*cut*/
            // newPos[0] = 0;    // r
            // newPos[1] = 0;   // p
            // newPos[2] = 0.6;     // y
            // newPos[3] = 2*(100/1000.0)+2*(z/1000.0)/5; // x
            // newPos[4] = 2*(20/1000.0); // y
            // newPos[5] = 2*(5/1000.0+y/1000/5) + 0.31; // z
        }
        
        {
            std::lock_guard<std::mutex> lk(posture_mutex);
            g_serverDesPosture = newPos;
        }
        
        // debug - 设置精度为小数点后6位
        std::cout << std::fixed << std::setprecision(6);
        std::cout << "[parseServerThreadFunc] server posture= " << newPos.transpose() << "\n";
        std::cout << std::defaultfloat; // 恢复默认格式（可选）
    }
}

// ==============================
// 你原有的 armControlThreadFunc
// ==============================
void armControlThreadFunc(Z1ARM& arm){
#ifdef USE_POSITION_CONTROL
    {
        std::lock_guard<std::mutex> lk(arm_mutex);
        arm.startTrack(ArmFSMState::JOINTCTRL);
    }
#endif

    // 新增的静态变量或全局变量，用于实现微调模式
    static bool microAdjustActive = false;  // 标记是否处于微调模式
    static Vec6 microAdjustBase;            // 记录进入微调模式时的机械臂基准位置

    while(!exit_flag){
        // 若没有新数据就 sleep
        if(!g_hasNewPacket.load()){
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            continue;
        }

        // 取最新数据
        std::vector<unsigned char> raw_packet;
        {
            std::lock_guard<std::mutex> lk(g_latestMutex);
            raw_packet = g_latestPacket;
            g_hasNewPacket.store(false);
        }

        // 数据长度检查
        if(raw_packet.size()<44) {
            continue;
        }

        float pos_array[7];
        double roll=0, pitch=0, yaw=0;
        memcpy(pos_array,&raw_packet[4],7*sizeof(float));
        float x = pos_array[0];
        float y = pos_array[1];
        float z = pos_array[2];
        float x0,y0,z0;
        roll   = pos_array[5];
        pitch  = pos_array[3];
        yaw    = pos_array[4];

        std::cout<<"[armControlThreadFunc] x="<<x<<",y="<<y<<",z="<<z
                 <<",pitch="<<pitch<<",yaw="<<yaw<<",roll="<<roll<<"\n";

        // 判断 pos_array[6] 是否大于阈值，若是则进入或保持微调模式
        if(pos_array[6] > thresh){
            // 如果还没进入过微调模式，则记录当前机械臂位置作为基准
            if(!microAdjustActive){
                {
                    std::lock_guard<std::mutex> lk(arm_mutex);
                    microAdjustBase = arm.lowstate->endPosture; // 记录当下姿态
                    x0=x; y0=y; z0=z;
                }
                microAdjustActive = true;
                std::cout << "[armControlThreadFunc] >>> Enter micro-adjust mode! <<<\n";
            }

            // 处于微调模式时：在 microAdjustBase 的基础上累加增量
            Vec6 new_position;
            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                // 以基准位置为起点
                new_position = microAdjustBase;

                // 对应 xyz 增量 (根据你的映射方式自行调整)
                new_position[3] = microAdjustBase[3] + 1*(z - z0)/1000.0f;
                new_position[4] = microAdjustBase[4] + 1*(x - x0)/1000.0f;
                new_position[5] = microAdjustBase[5] + 1*(y - y0)/1000.0f;

                // 对应 roll pitch yaw 增量
                new_position[0] = roll/10.0f;
                new_position[1] = pitch/10.0f;
                new_position[2] = yaw/10.0f;
            }
            std::cout << "target: " << new_position.transpose() << std::endl;

            // IK+插值下发
#ifdef USE_POSITION_CONTROL
            RotMat R= robo::rpyToRotMat(new_position[0], new_position[1], new_position[2]);
            Eigen::Vector3d p(new_position[3], new_position[4], new_position[5]);
            Eigen::MatrixXd Ttemp= robo::RpToTrans(R,p);
            HomoMat TDes= Ttemp;

            Vec6 qPast, qdPast, q_result;
            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                qPast= arm.lowstate->getQ();
                qdPast= arm.lowstate->getQd();
            }

            auto armModel= dynamic_cast<Z1Model*>(arm._ctrlComp->armModel);
            if(!armModel){
                std::cerr<<"[armControlThreadFunc] armModel is not Z1Model.\n";
                continue;
            }

#ifdef USE_PREDICTIVE_CONTROL
            int32_t delay_ms=0;
            memcpy(&delay_ms, &raw_packet[40], sizeof(int32_t));
            double dynamic_latency= delay_ms/1000.0;
            std::cout<<"[armControlThreadFunc] dynamic_latency="<<dynamic_latency<<" s.\n";

            Vec6 qFuture= qPast + qdPast* dynamic_latency;
            if(!armModel->inverseKinematics(TDes,qFuture,q_result,true)){
                std::cerr<<"IK failed with predictive.\n";
                continue;
            }
#else
            if(!armModel->inverseKinematics(TDes,qPast,q_result,true)){
                std::cerr<<"IK fail.\n";
                continue;
            }
#endif

            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                Vec6 currentQ= arm.lowstate->getQ();
                int duration=100;
                Timer timer(arm._ctrlComp->dt);
                Vec6 qd= (q_result- currentQ)/(duration*arm._ctrlComp->dt);

                for(int i=0;i<duration && !exit_flag;i++){
                    double t= (double)i/duration;
                    arm.q= currentQ*(1-t)+ q_result*t;
                    arm.qd= qd;
                    arm.setArmCmd(arm.q,arm.qd);
                    timer.sleep();
                }
                arm.printState();
            }
#endif // USE_POSITION_CONTROL
        }
        else {
            // pos_array[6] <= thresh，退出微调模式(若已在微调模式则退出)
            if(microAdjustActive){
                microAdjustActive = false;
                std::cout << "[armControlThreadFunc] <<< Exit micro-adjust mode! >>>\n";
            }
            // 仍采用原先的控制逻辑：直接将姿态映射到 new_position
            Vec6 new_position;
            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                new_position= arm.lowstate->endPosture;
                // 你的拟合公式
                new_position[3] = 4 * (z / 1000.0f);
                new_position[4] = 4 * (x / 1000.0f);
                new_position[5] = 2 * (y / 1000.0f ) + 0.31;

                new_position[0]= roll/10.0f;
                new_position[1]= pitch/10.0f;
                new_position[2]= yaw/10.0f;
            }
            std::cout << "target: " << new_position.transpose() << std::endl;

#ifdef USE_POSITION_CONTROL
            RotMat R= robo::rpyToRotMat(new_position[0], new_position[1], new_position[2]);
            Eigen::Vector3d p(new_position[3], new_position[4], new_position[5]);
            Eigen::MatrixXd Ttemp= robo::RpToTrans(R,p);
            HomoMat TDes= Ttemp;

            Vec6 qPast, qdPast, q_result;
            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                qPast= arm.lowstate->getQ();
                qdPast= arm.lowstate->getQd();
            }

            auto armModel= dynamic_cast<Z1Model*>(arm._ctrlComp->armModel);
            if(!armModel){
                std::cerr<<"[armControlThreadFunc] armModel is not Z1Model.\n";
                continue;
            }

#ifdef USE_PREDICTIVE_CONTROL
            int32_t delay_ms=0;
            memcpy(&delay_ms, &raw_packet[40], sizeof(int32_t));
            double dynamic_latency= delay_ms/1000.0;
            std::cout<<"[armControlThreadFunc] dynamic_latency="<<dynamic_latency<<" s.\n";

            Vec6 qFuture= qPast + qdPast* dynamic_latency;
            if(!armModel->inverseKinematics(TDes,qFuture,q_result,true)){
                std::cerr<<"IK failed with predictive.\n";
                continue;
            }
#else
            if(!armModel->inverseKinematics(TDes,qPast,q_result,true)){
                std::cerr<<"IK fail.\n";
                continue;
            }
#endif

            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                Vec6 currentQ= arm.lowstate->getQ();
                int duration=100;
                Timer timer(arm._ctrlComp->dt);
                Vec6 qd= (q_result- currentQ)/(duration*arm._ctrlComp->dt);

                for(int i=0;i<duration && !exit_flag;i++){
                    double t= (double)i/duration;
                    arm.q= currentQ*(1-t)+ q_result*t;
                    arm.qd= qd;
                    arm.setArmCmd(arm.q,arm.qd);
                    timer.sleep();
                }
                arm.printState();
            }
#endif // USE_POSITION_CONTROL
        }
    } // end while
}

// ==============================
// 你原有的速度控制线程
// ==============================
void armSpeedControlThreadFunc(Z1ARM& arm){
    {
        std::lock_guard<std::mutex> lk(arm_mutex);
        arm.startTrack(ArmFSMState::CARTESIAN);
    }

    double angular_vel=0.3, linear_vel=0.3;

    while(!exit_flag){
        if(!g_hasNewPacket.load()){
            std::this_thread::sleep_for(std::chrono::milliseconds(50));
            continue;
        }
        std::vector<unsigned char> raw_packet;
        {
            std::lock_guard<std::mutex> lk(g_latestMutex);
            raw_packet= g_latestPacket;
            g_hasNewPacket.store(false);
        }
        if(raw_packet.size()<40) continue;

        float px,py,pz;
        memcpy(&px, &raw_packet[4],sizeof(float));
        memcpy(&py, &raw_packet[8],sizeof(float));
        memcpy(&pz, &raw_packet[12],sizeof(float));

        Vec7 directions;
        directions<<0,0,0,(pz-100)>0?0.5:-0.5,(py>0)?0.5:-0.5,(px>0)?0.5:-0.5,0;

        {
            std::lock_guard<std::mutex> lk(arm_mutex);
            std::cout<<"[armSpeedControlThreadFunc] directions="<<directions.transpose()<<"\n";
            arm.cartesianCtrlCmd(directions,angular_vel,linear_vel);
            arm.printState();
        }
    }
}
#define USE_DYNAMICS_COMPENSATION
#define USE_OUTER_PID   // 定义此宏以启用外层 PID 控制

#ifdef USE_ADMITTANCE_CONTROL
void admittanceControlThreadFunc(Z1ARM& arm)
{
    // 1) 初始化：进入 LOWCMD
    {
        std::lock_guard<std::mutex> lk(arm_mutex);
        arm.backToStart();
        arm.setFsm(ArmFSMState::PASSIVE);
        arm.setFsm(ArmFSMState::LOWCMD);
        std::cout << "[admittanceCtrl] 已切换到 LOWCMD 模式.\n";
    }
    {
        // 内层 PD 增益
        std::vector<double> deviceKP = {20, 30, 30, 20, 15, 10};
        std::vector<double> deviceKD = {2000, 2000, 2000, 2000, 2000, 2000};
        arm._ctrlComp->lowcmd->setControlGain(deviceKP, deviceKD);
    }

    arm.setWait(false);

    double freq = 20.0;            
    double dt   = 1.0/freq;
    std::chrono::microseconds period_us((int)(1e6/freq));

    // ============ 导纳参数 (仅x方向) ============
    double Bx = 1500.0;
    double Kx = 50.0;
    double forceThreshold = 30.0; 
    double hysteresisBand = 5.0;

    // upperTrigger=35N, lowerTrigger=25N
    double upperTrigger = forceThreshold + hysteresisBand; 
    double lowerTrigger = forceThreshold - hysteresisBand; 

    std::cout << "[admittanceCtrl] freq="<<freq<<"Hz\n"
              << "  Bx="<<Bx<<", Kx="<<Kx
              << ", forceThreshold="<<forceThreshold
              << ", hysteresisBand="<<hysteresisBand<<"\n"
              << "  => EnterAdmittance if F>"<< upperTrigger
              << ", ExitAdmittance if F<"<< lowerTrigger <<" (持续10帧)\n";

    // 3) 获取当前末端位姿
    Vec6 xCmd, xCmdDot;
    {
        std::lock_guard<std::mutex> lk(arm_mutex);
        xCmd= arm.lowstate->endPosture;
        std::cout << "[admittanceCtrl] init posture="<< xCmd.transpose() <<"\n";
    }
    xCmdDot.setZero();

    // 4) 外层 PID
    Vec6 KpJ, KdJ, KiJ;
    KpJ << 1,1,1,1,1,1;  
    KdJ << 3,3,3,3,3,3;  
    KiJ << 0.05,0.05,0.05,0.05,0.05,0.05;

    static Vec6 errQIntegral= Vec6::Zero();

    // 5) 加速度估计
    static Vec6 oldQd= Vec6::Zero();
    static Vec6 qddFiltered= Vec6::Zero();
    double alpha=0.2;

    // 6) 速度缩放
    double speedScale=1.0;

    // 7) IK结果低通
    static Vec6 qDes_filtered= Vec6::Zero();
    {
        std::lock_guard<std::mutex> lk(arm_mutex);
        qDes_filtered = arm.lowstate->getQ();
    }
    double lambda=0.1;

    // 8) 限幅
    double dvx_max=0.005;
    double vx_max= 0.02;

    // 暂停
    static Vec6 freezeQ= Vec6::Zero();
    static bool lastPauseState= false;

    // === 导纳标志
    static bool inAdmittance= false;

    // === 锁定标志 & 锁定位置
    static bool locked= false;
    static double lockedPos= 999999.0;

    // === forceBelowCount: 当在导纳模式中, force < lowerTrigger 连续多少帧才退出
    static int forceBelowCount=0;  
    int forceBelowNeeded= 20;   // 需连续20帧力小于阈值才退出

    // ---------------------- 新增：力传感器滤波变量 ----------------------
    static double Fz_filtered = 0.0;  // 初始=0
    double alphaFilter = 0.9;        // 滤波权重(0.9=>较慢跟随raw)

    int cycleCount=0;
    std::cout<<"[admittanceCtrl] start loop.\n";

    while(!exit_flag){
        auto t_start= std::chrono::steady_clock::now();
        cycleCount++;

        bool nowPause= s_pause.load();

        if(!arm.lowstate){
            std::cerr<<"[admittanceCtrl] lowstate=null, wait.\n";
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            continue;
        }

        // A) 暂停逻辑
        if(nowPause && !lastPauseState){
            Vec6 qNow;
            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                qNow= arm.lowstate->getQ();
            }
            freezeQ= qNow;
            std::cout<<"[admCtrl] Pause ON => freezeQ="<< freezeQ.transpose() <<"\n";
        }
        lastPauseState= nowPause;

        if(nowPause){
            {
                std::lock_guard<std::mutex> lk(arm_mutex);
                arm.setArmCmd(freezeQ, Vec6::Zero(), Vec6::Zero());
            }
            auto used_us= std::chrono::steady_clock::now()- t_start;
            if(used_us< period_us){
                std::this_thread::sleep_for(period_us- used_us);
            }
            continue;
        }

        // B) 读取力(原始)
        float f6_local[6];
        {
            std::lock_guard<std::mutex> fk(force_mutex);
            for(int i=0; i<6; i++){
                f6_local[i]= force[i];
            }
        }
        double Fz_raw= f6_local[2];

        // B.1) 做一阶低通滤波
        Fz_filtered = alphaFilter * Fz_filtered 
                    + (1.0 - alphaFilter) * Fz_raw;

        double fMag= std::fabs(Fz_filtered);

        // C) 读取上位机指令
        Vec6 xDes;
        {
            std::lock_guard<std::mutex> lk(posture_mutex);
            xDes= g_serverDesPosture;
        }

        // ========== 状态切换 ==========

        // (1) locked状态 + fMag>upperTrigger => 进入导纳
        if(locked && fMag> upperTrigger){
            locked= false;
            inAdmittance= true;
            forceBelowCount=0;
            std::cout<<"[admCtrl] locked=>inAdmittance, f="<<fMag<<"\n";
        }

        // (2) 非导纳、非锁定, fMag>upperTrigger => 进入导纳
        if(!inAdmittance && !locked){
            if(fMag> upperTrigger){
                inAdmittance= true;
                locked= false;
                forceBelowCount=0;
                std::cout<<"[admCtrl] EnterAdmittance, fMag="<< fMag <<"\n";
            }
        }

        // (3) 在导纳中, 若 fMag< lowerTrigger => forceBelowCount++
        //     若 forceBelowCount>= forceBelowNeeded => exitAdmittance+lock
        if(inAdmittance){
            if(fMag< lowerTrigger){
                forceBelowCount++;
                if(forceBelowCount>= forceBelowNeeded){
                    inAdmittance= false;
                    locked= true;
                    lockedPos= xCmd[3];
                    xCmdDot[3]= 0.0;
                    std::cout<<"[admCtrl] ExitAdmittance+Lock, xSafe="<<lockedPos<<"\n";
                }
            } else {
                forceBelowCount=0;
            }
        }

        // (4) locked==true & "上位机指令< lockedPos" => 解锁
        {
            double xUser= xDes[3];
            if(locked && (xUser< lockedPos)){
                locked= false;
                std::cout<<"[admCtrl] user behind lockedPos => unlock\n";
            }
        }

        // =========== 更新 xCmd[3] ===========

        double x_now= xCmd[3];
        double vx_now= xCmdDot[3];
        double x_target= xDes[3];

        if(inAdmittance){
            // 导纳阶段 => 屏蔽上位机
            double xRef= x_now;  // 不前进
            double signFz= (Fz_filtered>=0.0)? +1.0:-1.0;
            double Fz_used= Fz_filtered - signFz* forceThreshold;

            double left= Bx*vx_now + Kx*(x_now - xRef);
            double dvx= dt*( Fz_used - left)/ Bx;

            if(dvx> dvx_max) dvx= dvx_max;
            if(dvx< -dvx_max) dvx= -dvx_max;
            xCmdDot[3]= vx_now + dvx;
            if(xCmdDot[3]> vx_max)  xCmdDot[3]= vx_max;
            if(xCmdDot[3]<-vx_max)  xCmdDot[3]=-vx_max;

            xCmd[3]= x_now + xCmdDot[3]*dt;
        }
        else if(locked){
            // 锁定：不允许 xCmd[3] < lockedPos => 保持 lockedPos
            //       允许 user 后退 => x_target> lockedPos => 跟随
            if(x_target> lockedPos){
                xCmd[3]= lockedPos;
                xCmdDot[3]=0.0;
            } else {
                xCmd[3]= x_target;
                xCmdDot[3]=0.0;
            }
        }
        else {
            // 正常：跟随上位机
            xCmdDot[3]=0.0;
            xCmd[3]= x_target;
        }

        // 其余维度
        xCmd[0]= xDes[0];
        xCmd[1]= xDes[1];
        xCmd[2]= xDes[2];
        xCmd[4]= xDes[4];
        xCmd[5]= xDes[5];

        // E) IK
        RotMat R= robo::rpyToRotMat(xCmd[0], xCmd[1], xCmd[2]);
        Eigen::Vector3d p(xCmd[3], xCmd[4], xCmd[5]);
        HomoMat TDes= robo::RpToTrans(R,p);

        Z1Model* z1= dynamic_cast<Z1Model*>( arm._ctrlComp->armModel );
        if(!z1){
            std::cerr<<"[admCtrl] armModel!=Z1Model.\n";
            break;
        }
        Vec6 qRes;
        {
            std::lock_guard<std::mutex> lk(arm_mutex);
            Vec6 qNow= arm.lowstate->getQ();
            bool ret= z1->inverseKinematics(TDes, qNow, qRes,true);
            if(!ret){
                std::cerr<<"[admCtrl] IK失败 xCmd="<< xCmd.transpose()<<"\n";
                auto used_us= std::chrono::steady_clock::now()- t_start;
                if(used_us<period_us){
                    std::this_thread::sleep_for(period_us- used_us);
                }
                continue;
            }
        }
        // IK 低通
        qDes_filtered= lambda*qRes + (1.0-lambda)* qDes_filtered;

#ifdef USE_OUTER_PID
        {
            std::lock_guard<std::mutex> lk(arm_mutex);
            Vec6 qNow= arm.lowstate->getQ();
            Vec6 qdNow= arm.lowstate->getQd();

            // 加速度
            Vec6 rawQdd= (qdNow- oldQd)/dt;
            for(int i=0; i<6; i++){
                qddFiltered[i]= alpha*qddFiltered[i] + (1.0-alpha)*rawQdd[i];
            }
            oldQd= qdNow;

            // 动补
            Vec6 tauFF= arm._ctrlComp->armModel->inverseDynamics(qNow,qdNow,qddFiltered,Vec6::Zero());
            Vec6 errQ= (qDes_filtered - qNow);
            Vec6 errQd= -qdNow;

            for(int i=0; i<6; i++){
                errQIntegral[i]+= errQ[i]*dt;
                double limit=0.5;
                if(errQIntegral[i]>limit) errQIntegral[i]=limit;
                if(errQIntegral[i]<-limit) errQIntegral[i]=-limit;
            }

            Vec6 tauOut;
            for(int i=0; i<6; i++){
                tauOut[i]= tauFF[i]
                         + KpJ[i]* errQ[i]
                         + KdJ[i]* errQd[i]
                         + KiJ[i]* errQIntegral[i];
            }

            Vec6 qdCmd= (qDes_filtered - qNow)* (1.0/dt)* speedScale;
            arm.setArmCmd(qDes_filtered, qdCmd, tauOut);
        }
#else
        {
            std::lock_guard<std::mutex> lk(arm_mutex);
            Vec6 qNow= arm.lowstate->getQ();
            Vec6 qdCmd= (qDes_filtered - qNow)* (1.0/dt)* speedScale;
            arm.setArmCmd(qDes_filtered, qdCmd, Vec6::Zero());
        }
#endif

        // F) Debug
        if((cycleCount%10)==0){
            std::cout<<"[admCtrl] cyc="<< cycleCount
                     <<", Fz(raw)="<< Fz_raw
                     <<", Fz_filtered="<<Fz_filtered
                     <<", inAdm="<< inAdmittance
                     <<", locked="<< locked
                     <<", lockedPos="<< lockedPos
                     <<", forceBelowCount="<< forceBelowCount
                     <<", xCmd="<< xCmd.transpose()<<"\n";
        }

        // Sleep
        auto t_end= std::chrono::steady_clock::now();
        auto used_us= std::chrono::duration_cast<std::chrono::microseconds>(
                          t_end - t_start);
        if(used_us< period_us){
            std::this_thread::sleep_for(period_us- used_us);
        }
    }
    std::cout<<"[admittanceCtrl] 退出.\n";
}
#endif // USE_ADMITTANCE_CONTROL



// ============= serial(串口) 读取力数据线程 =============
unsigned char cmd[] = "AT+GOD\r\n";
unsigned char response[100];  // Buffer for the response
void readSerialData(int fd, std::atomic<bool>& exit_flag){
    std::string buffer;
    char c;
    while(!exit_flag){
        tcflush(fd, TCIFLUSH);
        (void)write(fd, cmd, sizeof(cmd) - 1);
        usleep(10000);
        int bytes_read = read(fd, response, sizeof(response));
        if (bytes_read > 0) {
            // print_header_check(response, bytes_read);  // Print first 2 bytes for debugging
            // Check header
            if (response[0] == 0xAA && response[1] == 0x55) {
                parse_response(response, bytes_read);  // Process valid frame
            } else {
                cout << "Invalid header." << endl;
            }
        } else {
            cout << "No data received." << endl;
        }
        usleep(10000);  // Wait before sending the next command
    }
    close(fd);
}

// ============= 将力数据发往服务器的线程 =============
void sendForceThreadFunc(Z1ARM& arm) {
    while (!exit_flag) {
        float force_vals[6];
        {
            std::unique_lock<std::mutex> lk(force_mutex);
            std::copy(std::begin(force), std::end(force), std::begin(force_vals));
        }
        // debug
        //for (int i = 0; i < 6; i++) std::cout << force_vals[i] << " ";
        //std::cout << std::endl;

        sendForceData(arm, force_vals);
        usleep(10000);  // Sleep for 100 ms
    }
}


// Safe arm shutdown helper function
bool safeArmShutdown(Z1ARM& arm) {
    std::cout << "[safeArmShutdown] Moving arm to safe position...\n";
    
    // Use a timeout mechanism to prevent indefinite blocking
    std::atomic<bool> shutdownComplete(false);
    std::atomic<bool> timeoutOccurred(false);
    
    // Create a timeout thread
    std::thread timeoutThread([&]() {
        for (int i = 0; i < 50 && !shutdownComplete; i++) {
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
        }
        if (!shutdownComplete) {
            std::cerr << "[safeArmShutdown] WARNING: Arm restoration timed out!\n";
            timeoutOccurred.store(true);
        }
    });
    timeoutThread.detach();
    
    try {
        // Make arm movements blocking but with a safety net
        arm.setWait(true);
        
        // Try to get the arm to a safe position
        arm.backToStart();
        
        // Set to passive mode after reaching start
        arm.setFsm(ArmFSMState::PASSIVE);
        
        shutdownComplete = true;
        std::cout << "[safeArmShutdown] Arm successfully restored\n";
        return true;
    } 
    catch (const std::exception& e) {
        std::cerr << "[safeArmShutdown] Exception: " << e.what() << std::endl;
        return false;
    }
    catch (...) {
        std::cerr << "[safeArmShutdown] Unknown exception\n";
        return false;
    }
}
// Add this include at the top of your file
#include <csignal>

int main() {
    // Set up signal handling with proper error checking
    try {
        std::signal(SIGINT, [](int) {
            std::cout << "\n[main] SIGINT received, initiating graceful shutdown...\n";
            exit_flag = true;
            exit_flag_motor = true;
            force_cv.notify_all();
        });
    } catch (const std::exception& e) {
        std::cerr << "[main] Failed to set up signal handler: " << e.what() << "\n";
    }

    // Configure output formatting
    std::cout << std::fixed << std::setprecision(3);
    
    // Initialize force sensor serial port
    int serial_fd = initSerialPort("/dev/ttyUSB0");
    if (serial_fd < 0) {
        std::cerr << "[main] WARNING: Force sensor initialization failed, continuing without force sensing\n";
    } else {
        std::cout << "[main] Force sensor initialized on " << "/dev/ttyUSB0" << "\n";
    }
    
    // Initialize robot arm
    Z1ARM arm;
    if (arm.sendRecvThread != nullptr) {
        arm.sendRecvThread->start();
        std::cout << "[main] ARM communication thread started\n";
    } else {
        std::cerr << "[main] ERROR: ARM communication thread unavailable\n";
        return 1;  // Exit with error code
    }
    
    // Try to initialize arm to a known state
    try {
        std::cout << "[main] Moving arm to start position...\n";
        arm.backToStart();
        std::cout << "[main] Setting FSM to PASSIVE mode...\n";
        arm.setFsm(ArmFSMState::PASSIVE);
        std::cout << "[main] Setting FSM to LOWCMD mode...\n";
        arm.setFsm(ArmFSMState::LOWCMD);
        
        // Configure arm control gains
        std::cout << "[main] Configuring control gains...\n";
        std::vector<double> KP = arm._ctrlComp->lowcmd->kp;
        std::vector<double> KD = arm._ctrlComp->lowcmd->kd;
        arm._ctrlComp->lowcmd->setControlGain(KP, KD);
        
        // Return to PASSIVE mode after configuration
        arm.setFsm(ArmFSMState::PASSIVE);
        std::cout << "[main] Arm initialization complete\n";
    } catch (const std::exception& e) {
        std::cerr << "[main] Exception during arm initialization: " << e.what() << "\n";
        return 1;  // Exit with error code
    } catch (...) {
        std::cerr << "[main] Unknown exception during arm initialization\n";
        return 1;  // Exit with error code
    }
    
    // Start TCP client thread - connecting to remote server
    std::thread tcp_thread(tcpClient, std::ref(arm));
    std::cout << "[main] TCP client thread started\n";
    
    // Start keyboard monitoring thread
    std::thread keyboard_thread(monitorKeyboard, std::ref(exit_flag));
    std::cout << "[main] Keyboard monitor thread started\n";
    
    // Start motor control thread
    exit_flag_motor = false;  // Ensure motor thread starts clean
    std::thread motorThread(motorSerialThreadFunc);
    std::cout << "[main] Motor control thread started\n";
    
    // Wait for TCP connection to complete
    tcp_thread.join();
    std::cout << "[main] TCP connection established\n";
    
    // Check if shutdown was requested during TCP connection
    if (exit_flag) {
        std::cout << "[main] Shutdown requested during initialization\n";
        safeArmShutdown(arm);
        
        // Join remaining threads
        if (keyboard_thread.joinable()) keyboard_thread.join();
        if (motorThread.joinable()) motorThread.join();
        
        // Shutdown arm communication
        if (arm.sendRecvThread) {
            arm.sendRecvThread->shutdown();
        }
        
        return 0;
    }
    
    // Create log file with timestamp
    std::string filename;
    {
        auto t = std::time(nullptr);
        auto tmptr = std::localtime(&t);
        char buf[128];
        if (std::strftime(buf, sizeof(buf), "arm_position_log_%Y%m%d_%H%M%S.txt", tmptr) == 0) {
            std::cerr << "[main] Error creating log filename\n";
            exit_flag = true;
            safeArmShutdown(arm);
            return 1;
        }
        filename = buf;
        std::cout << "[main] Log file: " << filename << "\n";
    }
    
    // Collection of threads for easier management
    std::vector<std::thread> threads;
    
    // Declare all threads we'll need
    std::thread log_thread;
    std::thread arm_ctrl_thread;
    std::thread receive_thread;
    std::thread control_thread;
    std::thread serial_thread;
    std::thread force_send_thread;
    std::thread parse_thread;
    
    // Start logging thread if not in shutdown mode
    if (!exit_flag) {
        log_thread = std::thread(logArmPosition, std::ref(arm), std::ref(exit_flag), filename);
        std::cout << "[main] Logging thread started\n";
    }
    
    // Start control threads if TCP connection succeeded
    if (arm.tcp_sockfd != -1) {
        // Start real-time critical threads
        #ifdef USE_ADMITTANCE_CONTROL
            std::cout << "[main] Using ADMITTANCE control mode\n";
        #else
            arm_ctrl_thread = std::thread(&Z1ARM::armCtrlByFSM, &arm);
            std::cout << "[main] ARM control FSM thread started\n";
        #endif
        
        // Start TCP data handling threads
        receive_thread = std::thread(receiveDataFromServer, std::ref(arm));
        std::cout << "[main] TCP receive thread started\n";
        
        parse_thread = std::thread(parseServerThreadFunc);
        std::cout << "[main] Server data parsing thread started\n";
        
        // Start sensor and control threads
        if (serial_fd >= 0) {
            serial_thread = std::thread(readSerialData, serial_fd, std::ref(exit_flag));
            std::cout << "[main] Force sensor reading thread started\n";
        }
        
        force_send_thread = std::thread(sendForceThreadFunc, std::ref(arm));
        std::cout << "[main] Force data sending thread started\n";
        
        // Start appropriate control mode thread
        #ifdef USE_ADMITTANCE_CONTROL
            control_thread = std::thread(admittanceControlThreadFunc, std::ref(arm));
            std::cout << "[main] Admittance control thread started\n";
        #elif defined(USE_POSITION_CONTROL)
            control_thread = std::thread(armControlThreadFunc, std::ref(arm));
            std::cout << "[main] Position control thread started\n";
        #elif defined(USE_SPEED_CONTROL)
            control_thread = std::thread(armSpeedControlThreadFunc, std::ref(arm));
            std::cout << "[main] Speed control thread started\n";
        #else
            std::cout << "[main] WARNING: No control mode enabled\n";
        #endif
        
        std::cout << "[main] All threads started successfully, running...\n";
        
        // Main thread waits for exit signal with periodic health checks
        int healthCheckCounter = 0;
        while (!exit_flag) {
            // Wait with timeout to allow responding to signals
            std::this_thread::sleep_for(std::chrono::milliseconds(100));
            
            // Perform periodic health check (every ~5 seconds)
            if (++healthCheckCounter >= 50) {
                healthCheckCounter = 0;
                
                // Check if TCP connection is still alive
                if (arm.tcp_sockfd == -1) {
                    std::cerr << "[main] TCP connection lost, initiating shutdown\n";
                    exit_flag = true;
                    break;
                }
            }
        }
        
        // Begin graceful shutdown
        std::cout << "\n[main] *** Shutdown initiated ***\n";
        
        // Close network connection first to prevent hanging on socket operations
        if (arm.tcp_sockfd != -1) {
            std::cout << "[main] Closing TCP connection...\n";
            shutdown(arm.tcp_sockfd, SHUT_RDWR);  // More graceful than just close()
            close(arm.tcp_sockfd);
            arm.tcp_sockfd = -1;
        }
        
        // Wait briefly for threads to notice connection closure
        std::this_thread::sleep_for(std::chrono::milliseconds(200));
        
        // Signal all condition variables to unblock waiting threads
        force_cv.notify_all();
        
        // Join network and control related threads first
        std::cout << "[main] Waiting for network and control threads to exit...\n";
        
        if (receive_thread.joinable()) {
            receive_thread.join();
            std::cout << "[main] Receive thread joined\n";
        }
        
        if (parse_thread.joinable()) {
            parse_thread.join();
            std::cout << "[main] Parse thread joined\n";
        }
        
        if (control_thread.joinable()) {
            control_thread.join();
            std::cout << "[main] Control thread joined\n";
        }
        
        #ifndef USE_ADMITTANCE_CONTROL
        if (arm_ctrl_thread.joinable()) {
            arm_ctrl_thread.join();
            std::cout << "[main] ARM control FSM thread joined\n";
        }
        #endif
    } else {
        std::cerr << "[main] TCP connection failed, skipping control threads\n";
    }
    
    // Join remaining non-control threads
    force_cv.notify_all();
    
    if (force_send_thread.joinable()) {
        std::cout << "[main] Waiting for force thread to exit...\n";
        force_send_thread.join();
    }
    
    if (keyboard_thread.joinable()) {
        std::cout << "[main] Waiting for keyboard thread to exit...\n";
        keyboard_thread.join();
    }
    
    if (log_thread.joinable()) {
        std::cout << "[main] Waiting for logging thread to exit...\n";
        log_thread.join();
    }
    
    if (serial_thread.joinable()) {
        std::cout << "[main] Waiting for serial thread to exit...\n";
        serial_thread.join();
    }
    
    if (motorThread.joinable()) {
        std::cout << "[main] Waiting for motor thread to exit...\n";
        motorThread.join();
    }
    
    // Safe arm shutdown with robust error handling
    std::cout << "[main] Moving arm to safe position...\n";
    bool armShutdownSuccess = safeArmShutdown(arm);
    
    if (!armShutdownSuccess) {
        std::cerr << "[main] WARNING: Arm shutdown may not have completed properly\n";
    } else {
        std::cout << "[main] Arm shutdown completed successfully\n";
    }
    
    // Final cleanup - always attempt to shut down arm communication
    if (arm.sendRecvThread) {
        try {
            std::cout << "[main] Shutting down arm communication...\n";
            arm.sendRecvThread->shutdown();
            std::cout << "[main] Arm communication shutdown complete\n";
        } catch (const std::exception& e) {
            std::cerr << "[main] Exception during communication shutdown: " << e.what() << "\n";
        } catch (...) {
            std::cerr << "[main] Unknown exception during communication shutdown\n";
        }
    }
    
    std::cout << "[main] Clean exit complete\n";
    return 0;
}